#include "Geocentric/LocalCartesian.hpp"
#include <random>
#include "graph_optimizer/utility/data_types.hpp"
#include "graph_optimizer/edge/edge_gnss.hpp"
#include "graph_optimizer/edge/edge_og.hpp"
#include "graph_optimizer/optimizer/optimizer.hpp"
#include "graph_optimizer/global_defination/global_defination.h"
#define D2R 0.017453292519943295
using namespace graph_optimizer;

std::deque<GNSSData> gnss_buff;
std::deque<OgData> og_buff;
std::deque<PoseData> gt_buff;
std::deque<OgData> current_og;
GNSSData current_gnss;
PoseData current_gt;
PoseData current_pose;
ErrorState current_error_state;
GeographicLib::LocalCartesian geo_converter(32, 120, 0);
std::ofstream gt_ofs;
std::ofstream preint_ofs;
std::ofstream opt_ofs;
double optimize_interval = 10;
std::shared_ptr<Optimizer> optimizer;
int node_num = 0;
double optimize_time = 0;
bool ReadData(const std::vector<std::string> &path)
{
    gnss_buff.clear();
    og_buff.clear();
    gt_buff.clear();
    std::vector<std::ifstream> reads;
    for (int i = 0; i < path.size(); ++i)
    {
        reads.push_back(std::ifstream(path[i]));
    }
    for (int i = 0; i < path.size(); ++i)
    {
        std::string strs;
        std::getline(reads[i], strs);
    }
    std::string strs;
    while (std::getline(reads[0], strs))
    {
        double time = std::stod(strs);
        std::string temp;

        std::getline(reads[1], strs);
        temp = "";
        std::vector<double> gyro;
        for (int i = 0; i < strs.size(); ++i)
        {
            if (strs[i] == ',')
            {
                gyro.push_back(std::stod(temp));
                temp = "";
            }
            else
            {
                temp = temp + strs[i];
            }
        }
        gyro.push_back(std::stod(temp));

        OgData og;
        og.time = time;
        og.phi = Eigen::Vector3d(0, 0, 0);
        og.gyro = Eigen::Vector3d(gyro[0] * D2R, gyro[1] * D2R, gyro[2] * D2R);
        og_buff.push_back(og);

        std::getline(reads[4], strs);
        temp = "";
        std::vector<double> ref_pos;
        for (int i = 0; i < strs.size(); ++i)
        {
            if (strs[i] == ',')
            {
                ref_pos.push_back(std::stod(temp));
                temp = "";
            }
            else
            {
                temp = temp + strs[i];
            }
        }
        ref_pos.push_back(std::stod(temp));

        std::getline(reads[5], strs);
        temp = "";
        std::vector<double> ref_att_quat;
        for (int i = 0; i < strs.size(); ++i)
        {
            if (strs[i] == ',')
            {
                ref_att_quat.push_back(std::stod(temp));
                temp = "";
            }
            else
            {
                temp = temp + strs[i];
            }
        }
        ref_att_quat.push_back(std::stod(temp));

        Eigen::Quaterniond q = Eigen::AngleAxisd(90 * D2R, Eigen::Vector3d::UnitZ()) *
                               Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY()) *
                               Eigen::AngleAxisd(180 * D2R, Eigen::Vector3d::UnitX());
        q = q.inverse();

        PoseData pose;
        pose.time = time;
        double geo_x, geo_y, geo_z;
        geo_converter.Forward(ref_pos[0], ref_pos[1], ref_pos[2], geo_x, geo_y, geo_z);
        pose.state.p = Eigen::Vector3d(geo_x, geo_y, geo_z);
        pose.state.q = q * Eigen::Quaterniond(ref_att_quat[0], ref_att_quat[1], ref_att_quat[2], ref_att_quat[3]);
        pose.state.q.normalize();
        pose.state.bg = Eigen::Vector3d(0, 0, 0);
        gt_buff.push_back(pose);

        if (gt_buff.size() > 1)
        {
            unsigned int kk = gt_buff.size();
            Eigen::Vector3d dp = gt_buff[kk - 2].state.q.inverse() * (gt_buff[kk - 1].state.p - gt_buff[kk - 2].state.p);
            double phi = dp[1];
            // std::random_device rd;
            // std::default_random_engine generator(rd());
            // std::normal_distribution<double> distribution(0.0, 1.0);
            // phi = phi + phi_noise * distribution(generator);
            og_buff[kk - 2].phi = Eigen::Vector3d(0, phi, 0);
            og_buff[kk - 1].phi = og_buff[kk - 2].phi;
        }
    }

    while (std::getline(reads[2], strs))
    {
        double time = std::stod(strs);
        std::getline(reads[3], strs);
        std::string temp = "";
        std::vector<double> gps;
        for (int i = 0; i < strs.size(); ++i)
        {
            if (strs[i] == ',')
            {
                gps.push_back(std::stod(temp));
                temp = "";
            }
            else
            {
                temp = temp + strs[i];
            }
        }
        gps.push_back(std::stod(temp));
        GNSSData gnss;
        gnss.time = time;
        gnss.lla = Eigen::Vector3d(gps[0], gps[1], gps[2]);
        gnss_buff.push_back(gnss);
    }
}

bool SyncData(bool inited)
{
    if (gnss_buff.empty())
    {
        return false;
    }
    current_gnss = gnss_buff.front();
    double sync_time = current_gnss.time;

    while (gt_buff.size() > 1)
    {
        if (gt_buff[1].time < sync_time)
        {
            gt_buff.pop_front();
        }
        else
        {
            break;
        }
    }

    if (gt_buff.size() > 1)
    {
        PoseData front_data = gt_buff.at(0);
        PoseData back_data = gt_buff.at(1);
        double front_scale = (back_data.time - sync_time) / (back_data.time - front_data.time);
        double back_scale = (sync_time - front_data.time) / (back_data.time - front_data.time);
        current_gt.time = sync_time;
        current_gt.state.p = front_data.state.p * front_scale + back_data.state.p * back_scale;
        current_gt.state.q = front_data.state.q.slerp(front_scale, back_data.state.q);
        current_gt.state.bg = front_data.state.bg * front_scale + back_data.state.bg * back_scale;
    }
    else
    {
        return false;
    }

    while (!inited && og_buff.size() > 1)
    {
        if (og_buff[1].time < sync_time)
        {
            og_buff.pop_front();
        }
        else
        {
            break;
        }
    }

    if (og_buff.size() > 1)
    {
        if (!inited)
        {
            current_og.clear();
            OgData front_data = og_buff.at(0);
            OgData back_data = og_buff.at(1);
            OgData synced_data;

            double front_scale = (back_data.time - sync_time) / (back_data.time - front_data.time);
            double back_scale = (sync_time - front_data.time) / (back_data.time - front_data.time);
            synced_data.time = sync_time;
            synced_data.phi = front_data.phi * front_scale + back_data.phi * back_scale;
            synced_data.gyro = front_data.gyro * front_scale + back_data.gyro * back_scale;
            current_og.push_back(synced_data);
            og_buff.pop_front();
            gnss_buff.pop_front();
            // std::cout << std::setprecision(12) << "sync_time " << sync_time
            //           << " current_og.time " << current_og.front().time
            //           << "  " << current_og.back().time << std::endl;
            return true;
        }

        if (og_buff.back().time < sync_time)
        {
            return false;
        }
        while (current_og.size() > 1)
        {
            current_og.pop_front();
        }
        while (og_buff.front().time < sync_time)
        {
            OgData temp = og_buff.front();
            og_buff.pop_front();
            current_og.push_back(temp);
        }
        OgData front_data = current_og.back();
        OgData back_data = og_buff.at(0);
        OgData synced_data;

        double front_scale = (back_data.time - sync_time) / (back_data.time - front_data.time);
        double back_scale = (sync_time - front_data.time) / (back_data.time - front_data.time);
        synced_data.time = sync_time;
        synced_data.phi = front_data.phi * front_scale + back_data.phi * back_scale;
        synced_data.gyro = front_data.gyro * front_scale + back_data.gyro * back_scale;
        current_og.push_back(synced_data);
        gnss_buff.pop_front();
        // std::cout << std::setprecision(12) << "sync_time " << sync_time
        //           << " current_og.time " << current_og.front().time
        //           << "  " << current_og.back().time << std::endl;
        return true;
    }
    else
    {
        return false;
    }
}

bool InitSensor()
{
    while (!gnss_buff.empty())
    {
        if (og_buff.front().time > gnss_buff.front().time)
        {
            gnss_buff.pop_front();
        }
        else
        {
            return true;
        }
    }
    return false;
}

Eigen::Vector3d LlaToLocal(const GNSSData &gnss_data)
{
    double geo_x, geo_y, geo_z;
    geo_converter.Forward(gnss_data.lla[0], gnss_data.lla[1], gnss_data.lla[2], geo_x, geo_y, geo_z);
    return Eigen::Vector3d(geo_x, geo_y, geo_z);
}

bool InitPose()
{
    static bool pose_inited = false;
    if (pose_inited)
    {
        return true;
    }
    if (!SyncData(false))
    {
        return false;
    }
    current_pose.time = current_gnss.time;
    current_pose.state.p = LlaToLocal(current_gnss);
    current_pose.state.q = current_gt.state.q;
    current_pose.state.bg = current_gt.state.bg;
    optimizer = std::make_shared<Optimizer>("lm_var");
    optimizer->AddNode(current_pose);
    optimizer->AddGnssEdge(0, current_pose.state.p);
    node_num = 0;
    optimize_time = current_gnss.time;
    return true;
}

bool Fuse()
{
    PoseData temp;
    temp.time = current_gnss.time;
    std::shared_ptr<PreIntegration> pre_integration = std::make_shared<PreIntegration>(current_og, current_pose.state.bg);
    pre_integration->propagate();
    temp.state.p = current_pose.state.p + current_pose.state.q * pre_integration->alpha_;
    temp.state.q = current_pose.state.q * pre_integration->dq_;
    temp.state.bg = current_pose.state.bg;
    int node_num2 = optimizer->GetNodeNum();
    optimizer->AddNode(temp);
    Eigen::Vector3d gpsxyz = LlaToLocal(current_gnss);
    optimizer->AddGnssEdge(node_num2, gpsxyz);
    optimizer->AddOgEdge(node_num, node_num2, pre_integration);
    node_num = node_num2;
    current_pose = temp;
}

bool Optimize(bool force)
{
    if (current_pose.time - optimize_time > optimize_interval || force)
    {
        optimizer->Optimize();
        optimize_time = current_pose.time;
    }
    return true;
}

void SavePose(std::ofstream &save_points, PoseData &pose)
{
    // Eigen::Quaterniond qtemp = pose.state.q;
    // double angle = std::acos(qtemp.w()) * 2;
    // double sin_angle = std::sin(angle / 2);
    // Eigen::Vector3d dir(0, 0, 0);
    // if (sin_angle != 0)
    // {
    //     dir(0) = qtemp.x() / sin_angle;
    //     dir(1) = qtemp.y() / sin_angle;
    //     dir(2) = qtemp.z() / sin_angle;
    //     dir = dir * angle;
    // }
    save_points.precision(12);
    save_points << pose.time
                << " " << pose.state.p(0)
                << " " << pose.state.p(1)
                << " " << pose.state.p(2)
                << " " << pose.state.q.x()
                << " " << pose.state.q.y()
                << " " << pose.state.q.z()
                << " " << pose.state.q.w()
                // << " " << dir(0)
                // << " " << dir(1)
                // << " " << dir(2)
                // << " " << pose.state.bg(0)
                // << " " << pose.state.bg(1)
                // << " " << pose.state.bg(2)
                << std::endl;
}

bool SaveData()
{
    SavePose(gt_ofs, current_gt);
    SavePose(preint_ofs, current_pose);
}

int main(int argc, char **argv)
{
    std::cout << WORK_SPACE_PATH << std::endl;
    YAML::Node yaml_node = YAML::LoadFile(WORK_SPACE_PATH + "/config/param.yaml");
    std::string directory = yaml_node["directory"].as<std::string>();
    gps_noise = yaml_node["gps_noise"].as<double>();
    gyro_noise = yaml_node["gyro_noise"].as<double>();
    phi_noise = yaml_node["phi_noise"].as<double>();
    optimize_interval = yaml_node["optimize_interval"].as<double>();

    std::vector<std::string> path;
    path.push_back(directory + "time.csv");
    path.push_back(directory + "gyro-0.csv");
    path.push_back(directory + "gps_time.csv");
    path.push_back(directory + "gps-0.csv");
    path.push_back(directory + "ref_pos.csv");
    path.push_back(directory + "ref_att_quat.csv");
    ReadData(path);
    gt_ofs.open(directory + "gt.txt", std::fstream::out);
    preint_ofs.open(directory + "preint.txt", std::fstream::out);
    opt_ofs.open(directory + "optimized.txt", std::fstream::out);

    if (!InitSensor())
    {
        std::cerr << "InitSensor Error!!!" << std::endl;
        return -1;
    }
    if (!InitPose())
    {
        std::cerr << "InitPose Error!!!" << std::endl;
        return -1;
    }
    SaveData();
    while (SyncData(true))
    {
        Fuse();
        Optimize(false);
        SaveData();
    }
    Optimize(true);
    std::deque<PoseData> opt_pose;
    optimizer->GetOptimizedPose(opt_pose);
    for (int i = 0; i < opt_pose.size(); ++i)
    {
        SavePose(opt_ofs, opt_pose[i]);
    }
    return 0;
}